#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import rospy
import numpy as np

from img_utils import ros_to_cv2

from sensor_msgs.msg import Image


class ImgDepth():
    def __init__(self):
        # 深度图像订阅者
        self.depth_image_sub = rospy.Subscriber('/camera/depth/image', Image, callback=self.depth_image_callback, queue_size=1)

        rospy.spin()

    def depth_image_callback(self, data):
        rospy.logdebug('depth image callback...')

        depth_img = ros_to_cv2(data)
        depth_array = np.array(depth_img, dtype=np.float32)
        rospy.loginfo('depth_array is: %s', depth_array)


if __name__ == '__main__':
    rospy.init_node('npu_robot_img_depth', anonymous=True, log_level=envs.log_level)
    img_depth = ImgDepth()
